#include "VirtualCamera.h"

//Needed for SetFrustum()
#include "MathHelper.h"

VirtualCamera::VirtualCamera()
{
	//Init position and the right, up, look vectors. 
	position = XMFLOAT3(0.0f, 0.0f, 0.0f);
	right = XMFLOAT3(1.0f, 0.0f, 0.0f);
	up = XMFLOAT3(0.0f, 1.0f, 0.0f);
	look = XMFLOAT3(0.0f, 0.0f, 1.0f);

	//Init projection matrix. 
	nearZ = 1.0f;
	farZ = 1000.0f;
	aspect = 1.0f;
	fovY = 0.25f*MathHelper::Pi;
	SetFrustumProperties(fovY, aspect, nearZ, farZ);

	//Initially, no rotations.
	rotationsX = 0.0f;
	rotationsY = 0.0f;
}

VirtualCamera::~VirtualCamera()
{}

XMVECTOR VirtualCamera::GetPositionXM()const
{
	return XMLoadFloat3(&position);
}

XMFLOAT3 VirtualCamera::GetPosition()const
{
	return position;
}

void VirtualCamera::SetPosition(float x, float y, float z)
{
	position = XMFLOAT3(x, y, z);
}

void VirtualCamera::SetPosition(XMVECTOR &vec)
{
	XMStoreFloat3(&position, vec);
}

void VirtualCamera::SetPosition(XMFLOAT3 &vec)
{
	position = vec;
}
 
XMVECTOR VirtualCamera::GetRightXM()const
{
	return XMLoadFloat3(&right);
}

XMFLOAT3 VirtualCamera::GetRight()const
{
	return right;
}

XMVECTOR VirtualCamera::GetUpXM()const
{
	return XMLoadFloat3(&up);
}

XMFLOAT3 VirtualCamera::GetUp()const
{
	return up;
}

XMVECTOR VirtualCamera::GetLookXM()const
{
	return XMLoadFloat3(&look);
}

XMFLOAT3 VirtualCamera::GetLook()const
{
	return look;
}

float VirtualCamera::GetNearZ()const
{
	return nearZ;
}

float VirtualCamera::GetFarZ()const
{
	return farZ;
}

float VirtualCamera::GetAspect()const
{
	return aspect;
}

float VirtualCamera::GetFovY()const
{
	return fovY;
}

float VirtualCamera::GetFovX()const
{
	float half =  0.5f*GetNearWindowWidth();
	return 2.0f*atan(half / nearZ);
}

float VirtualCamera::GetNearWindowWidth()const
{
	return aspect * nearWindowHeight;
}

float VirtualCamera::GetNearWindowHeight()const
{
	return nearWindowHeight;
}

float VirtualCamera::GetFarWindowWidth()const
{
	return aspect * farWindowHeight;
}

float VirtualCamera::GetFarWindowHeight()const
{
	return farWindowHeight;
}

void VirtualCamera::SetFrustumProperties(float fovY, float aspect, float nearZ, float farZ)
{
	//Store properties.
	this->fovY   = fovY;
	this->aspect = aspect;
	this->nearZ  = nearZ;
	this->farZ   = farZ;

	nearWindowHeight = 2.0f * nearZ * tanf(0.5f*fovY);
	farWindowHeight =  2.0f * farZ  * tanf(0.5f*fovY);

	XMMATRIX p = XMMatrixPerspectiveFovLH(fovY, aspect, nearZ, farZ);
	XMStoreFloat4x4(&proj, p);

	//Update frustum volume. 
	//We use an XNA Collision function to do the work for us. 
	XNA::ComputeFrustumFromProjection(&frustumVolume, &p);
}

void VirtualCamera::SetCameraProperties(FXMVECTOR pos, FXMVECTOR target, FXMVECTOR worldUp)
{
	OutputDebugStringW(L"Virtual Camera Error - SetCameraProperties() has been disabled...\n");
	while(1){}

	XMVECTOR l = XMVector3Normalize(XMVectorSubtract(target, pos));
	XMVECTOR r = XMVector3Normalize(XMVector3Cross(worldUp, l));
	XMVECTOR u = XMVector3Cross(l, r);

	XMStoreFloat3(&position, pos);
	XMStoreFloat3(&look, l);
	XMStoreFloat3(&right, r);
	XMStoreFloat3(&up, u);
}

void VirtualCamera::SetCameraProperties(const XMFLOAT3& pos, const XMFLOAT3& target, const XMFLOAT3& worldUp)
{
	XMVECTOR p = XMLoadFloat3(&pos);
	XMVECTOR t = XMLoadFloat3(&target);
	XMVECTOR u = XMLoadFloat3(&worldUp);

	SetCameraProperties(p, t, u);
}

XMMATRIX VirtualCamera::GetView()const
{
	return XMLoadFloat4x4(&view);
}

XMFLOAT4X4 VirtualCamera::GetViewFloat4x4()const
{
	return view;
}

XMMATRIX VirtualCamera::GetProjection()const
{
	return XMLoadFloat4x4(&proj);
}

XMFLOAT4X4 VirtualCamera::GetProjectionFloat4x4()const
{
	return proj;
}

XMMATRIX VirtualCamera::GetViewProjection()const
{
	return (XMMatrixMultiply(GetView(), GetProjection()));
}

XMFLOAT4X4 VirtualCamera::GetViewProjectionFloat4x4()const
{
	XMMATRIX vp = GetViewProjection();

	XMFLOAT4X4 ret;
	XMStoreFloat4x4(&ret, vp);

	return ret;
}

void VirtualCamera::Strafe(float distance)
{
	XMVECTOR s = XMVectorReplicate(distance);
	XMVECTOR r = XMLoadFloat3(&right);
	XMVECTOR p = XMLoadFloat3(&position);

	XMStoreFloat3(&position, XMVectorMultiplyAdd(s, r, p));
}

void VirtualCamera::Walk(float distance)
{
	XMVECTOR s = XMVectorReplicate(distance);
	XMVECTOR l = XMLoadFloat3(&look);
	XMVECTOR p = XMLoadFloat3(&position);

	XMStoreFloat3(&position, XMVectorMultiplyAdd(s, l, p));
}

void VirtualCamera::SetLook(XMFLOAT3 l)
{
	look = l;
}

void VirtualCamera::AddPitch(float angleXDegrees)
{
	//Rotate up and look vector about the right vector.
	XMMATRIX r = XMMatrixRotationAxis(XMLoadFloat3(&right), XMConvertToRadians(angleXDegrees));

	XMStoreFloat3(&up, XMVector3TransformNormal(XMLoadFloat3(&up), r));
	XMStoreFloat3(&look, XMVector3TransformNormal(XMLoadFloat3(&look), r));

	//Update our instance variable tracking x rotations. 
	rotationsX+=angleXDegrees;

	//Clamp within 0->360 range.
	if (rotationsX > 360.0f)
		rotationsX -= 360.0f;
	else if (rotationsX < 0.0f)
		rotationsX += 360.0f;
}

void VirtualCamera::AddYaw(float angleYDegrees)
{
	//Rotate the basis vector about the world axis (y)
	XMMATRIX r = XMMatrixRotationY(XMConvertToRadians(angleYDegrees));

	XMStoreFloat3(&right, XMVector3TransformNormal(XMLoadFloat3(&right), r));
	XMStoreFloat3(&up, XMVector3TransformNormal(XMLoadFloat3(&up), r));
	XMStoreFloat3(&look, XMVector3TransformNormal(XMLoadFloat3(&look), r));

	//Update our instance variable tracking Y rotations. 
	rotationsY+=angleYDegrees;

	//Clamp within 0->360 range.
	if (rotationsY > 360.0f)
		rotationsY -= 360.0f;
	else if (rotationsY < 0.0f)
		rotationsY += 360.0f;
}

void VirtualCamera::RebuildViewMatrix()
{
	XMVECTOR r = XMLoadFloat3(&right);
	XMVECTOR u = XMLoadFloat3(&up);
	XMVECTOR l = XMLoadFloat3(&look);
	XMVECTOR p = XMLoadFloat3(&position);

	//Keep cameras axis orthogonal to each other and normalized
	l = XMVector3Normalize(l);
	u = XMVector3Normalize(XMVector3Cross(l, r));

	//update right vector - return will already be normalized because u and l
	//have been above.
	r = XMVector3Cross(u, l);

	//Fill in view matrix entries.
	float x = -XMVectorGetX(XMVector3Dot(p, r));
	float y = -XMVectorGetX(XMVector3Dot(p, u));
	float z = -XMVectorGetX(XMVector3Dot(p, l));

	XMStoreFloat3(&right, r);
	XMStoreFloat3(&up, u);
	XMStoreFloat3(&look, l);

	view(0,0) = right.x; 
	view(1,0) = right.y; 
	view(2,0) = right.z; 
	view(3,0) = x;   

	view(0,1) = up.x;
	view(1,1) = up.y;
	view(2,1) = up.z;
	view(3,1) = y;  

	view(0,2) = look.x; 
	view(1,2) = look.y; 
	view(2,2) = look.z; 
	view(3,2) = z;   

	view(0,3) = 0.0f;
	view(1,3) = 0.0f;
	view(2,3) = 0.0f;
	view(3,3) = 1.0f;
}

void VirtualCamera::ReturnFrustumInLocalSpace(XMFLOAT4X4* objectWorld, XNA::Frustum* localSpaceFrustum)
{
	//Load the passed in XMFLOAT4X4 in to an XMMATRIX.
	XMMATRIX w = XMLoadFloat4x4(objectWorld);
	//Work out the inverse of the world transformation. 
	XMMATRIX worldInv = XMMatrixInverse(&XMMatrixDeterminant(w), w);

	//Also work out the inverse of the view matrix.
	XMMATRIX v = XMLoadFloat4x4(&view);
	XMMATRIX viewInv = XMMatrixInverse(&XMMatrixDeterminant(v), v);

	//view space to objects local space.
	XMMATRIX toLocal = XMMatrixMultiply(viewInv, worldInv);

	//Decompose the toLocal matrix. 
	XMVECTOR scale;
	XMVECTOR rotQuat;
	XMVECTOR trans;
	XMMatrixDecompose(&scale, &rotQuat, &trans, toLocal);

	//Transform the (out) frustum from view space to the objects local space.
	XNA::TransformFrustum(localSpaceFrustum, &frustumVolume, XMVectorGetX(scale), rotQuat, trans);
}

#include <iostream>
#include <sstream>

float VirtualCamera::GetCameraRotationDegreesPitch()
{
	//See: stackoverflow.com/questions/2396087/direct3d-how-do-i-calculate-roll-from-view-matrix
	//float lz = look.z;
	//float lx = look.x;

	//float lookLengthOnXZ = sqrtf((lz*lz) + (lx*lx));

	////Convert to degrees.
	//float ret = MathHelper::ToDegrees(atan2f(look.y, lookLengthOnXZ));

	//Easier just to use my trackers...
	float ret = rotationsX;

	std::stringstream ss;
	ss << ret;
	OutputDebugStringW(L"Cam Pitch = ");
	OutputDebugStringA(ss.str().c_str());
	OutputDebugStringW(L"\n");

	return ret;
}

float VirtualCamera::GetCameraRotationDegreesYaw()
{
	//See: stackoverflow.com/questions/2396087/direct3d-how-do-i-calculate-roll-from-view-matrix
	//float lz = look.z;
	//float lx = look.x;
	//float lookLengthOnXZ = sqrtf((lz*lz) + (lx*lx));

	////We want degrees.
	//float ret =  MathHelper::ToDegrees(atan2f(look.x, look.z));

	//Easier just to use my trackers...
	float ret = rotationsY;

	//std::stringstream ss;
	//ss << ret;
	//OutputDebugStringW(L"Cam Yaw = ");
	//OutputDebugStringA(ss.str().c_str());
	//OutputDebugStringW(L"\n");

	return ret;
}